import numpy as np
import pybullet as p

import constants as c
import pyrosim.pyrosim as pyrosim


class MOTOR:
    
    
    def __init__(self, jointName):
        
        self.__jointName = jointName
        self.__values = []
        
        
    def set_value(self, body, target_angle):
        
        self.__values.append(target_angle)
        pyrosim.Set_Motor_For_Joint(bodyIndex=body,
                                    jointName=self.__jointName,
                                    controlMode=p.POSITION_CONTROL,
                                    targetPosition=target_angle,
                                    maxForce=c.max_force)


    def save_values(self, task):
        
        self.__values = np.array(self.__values)
        np.save(f"logs/{task}-{self.__jointName}.npy", self.__values)
